//Includes all the headers necessary to use the most common public pieces of the ROS system.
#include <ros/ros.h>
#include "std_msgs/String.h"
#include <geometry_msgs/Twist.h> //Velocity vector
#include <clearpath_horizon/RawEncoders.h>
#include <indoor_pos/ips_msg.h>
#include <sstream>

void encoderCallback(const clearpath_horizon::RawEncodersConstPtr &msg)
{
	ROS_INFO("ENCODER: I got: [%d] as encoder ticks", msg->ticks[0]);
}

//UNCOMMENT THIS FUNCTION WHEN WORKING WITH IPS
/* void ipsCallback(const indoor_pos::ips_msg::ConstPtr& msgIPS)
{
	ROS_INFO("IPS:I got: X = %f, Y = %f, Yaw = %f", msgIPS->X, msgIPS->Y, msgIPS->Yaw);
}
*/

int main(int argc, char **argv)
{

	ros::init(argc,argv,"sampleCode");
	ROS_INFO("Initializing node: sampleCode");

	geometry_msgs::Twist velocityCommand;

	ros::NodeHandle nh;

	//UNCOMMENT THE FOLLOWING LINE WHEN WORKING WITH IPS	
	//ros::Subscriber subIPS = nh.subscribe("indoor_pos",1000,ipsCallback);

	ros::Subscriber subEncoder = nh.subscribe("/data/raw_encoders",1000,encoderCallback);

	ros::Publisher pubVelocity = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1000);


	//main loop
	ros::Rate loop_rate(20);		//20Hz update rate
	while (ros::ok())
	{
		velocityCommand.linear.x = 100;
		velocityCommand.angular.z = -50;
		pubVelocity.publish(velocityCommand);
		ros::spinOnce();	
		loop_rate.sleep();			//used for maintaining rate
	}
	return 0;
}

